#include <cstdlib>
#include <iostream>
#include <string>

#include <itkImage.h>
#include <itkImageFileReader.h>
#include <itkImageFileWriter.h>

#include <itkResampleImageFilter.h>
#include <itkRigid2DTransform.h>
#include <itkLinearInterpolateImageFunction.h>
#include <itkNearestNeighborInterpolateImageFunction.h>
#include <itkAddImageFilter.h>

#include <itkImageRegionConstIteratorWithIndex.h>
#include <itkImageRegionIteratorWithIndex.h>

#include <extImageMoments.h>
#include <rgcContour.h>
#include <frLineSpatialObject.h>

#include <vtkAxesActor.h>
#include <vtkImageActor.h>
#include <vtkImageMapToColors.h>
#include <vtkInteractorStyleImage.h>
#include <vtkLookupTable.h>
#include <vtkMatrix4x4.h>
#include <vtkRenderer.h>
#include <vtkRenderWindow.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkSmartPointer.h>

#include <itkImageToVTKImageFilter.h>
#include <rgc_vtkContourActor.h>

const unsigned int Dim = 2;
typedef double     TPixel;
typedef double     TScalar;

typedef itk::Image< TPixel, Dim >      TImage;
typedef itk::ImageFileReader< TImage > TImageReader;
typedef itk::ImageFileWriter< TImage > TImageWriter;

typedef itk::ResampleImageFilter< TImage, TImage, TScalar >    TResample;
typedef itk::Rigid2DTransform< TScalar >                       TTransform;
typedef itk::LinearInterpolateImageFunction< TImage, TScalar > TLinearInt;
typedef
itk::NearestNeighborInterpolateImageFunction< TImage, TScalar > TNearInt;

typedef rgc::Contour< TScalar > TContour;
typedef TContour::TComplex      TComplex;

typedef ext::ImageMoments< TImage, TScalar > TMoments;
typedef TMoments::TMatrix TMatrix;
typedef TMoments::TPoint  TPoint;
typedef TMoments::TVector TVector;

typedef fr::LineSpatialObject< TPoint > TLine;

typedef itk::ImageToVTKImageFilter< TImage >         TITK2VTK;
typedef vtkSmartPointer< vtkAxesActor >              TAxesActor;
typedef vtkSmartPointer< vtkMatrix4x4 >              TMatrix4x4;
typedef vtkSmartPointer< vtkImageActor >             TImageActor;
typedef vtkSmartPointer< vtkRenderWindowInteractor > TInteractor;
typedef vtkSmartPointer< rgc::vtkContourActor >      TContourActor;
typedef vtkSmartPointer< vtkLookupTable >            TLookupTable;
typedef vtkSmartPointer< vtkImageMapToColors >       TImageMapToColors;
typedef vtkSmartPointer< vtkRenderer >               TRenderer;
typedef vtkSmartPointer< vtkRenderWindow >           TRenderWindow;
typedef vtkSmartPointer< vtkRenderWindowInteractor > TInteractor;
// typedef vtkSmartPointer< vtkInteractorStyleImage >   TInteractor;


// -------------------------------------------------------------------------
void ComputeMoments(
  TContour::Pointer& ellipse, TVector& cog, TMatrix& inertia,
  const TImage* image, const TPoint* p0 = NULL, const TPoint* p1 = NULL
  );

void CutImage(
  TImage::Pointer& cut,
  const TImage* image, const TContour* ellipse,
  const TPoint& p0, const TPoint& p1, double angle
  );

// -------------------------------------------------------------------------
int main( int argc, char* argv[] )
{
  if( argc < 4 )
  {
    std::cerr
      << "Usage: " << argv[ 0 ]
      << " input_image_file"
      << " output_image_file angle" << std::endl;
    return( 1 );

  } // fi
  std::string input_image_file = argv[ 1 ];
  std::string output_image_file = argv[ 2 ];
  double angle = std::atof( argv[ 3 ] );

  // Read image
  TImageReader::Pointer input_image_reader = TImageReader::New( );
  input_image_reader->SetFileName( input_image_file );
  try
  {
    input_image_reader->Update( );
  }
  catch( itk::ExceptionObject& err )
  {
    std::cout
      << "Exception object caught: "
      << err
      << std::endl; 
    return( -1 );

  } // yrt
  TImage::ConstPointer input_image = input_image_reader->GetOutput( );

  // Compute global moments
  TContour::Pointer global_ellipse;
  TVector global_cog;
  TMatrix global_inertia;
  ComputeMoments( global_ellipse, global_cog, global_inertia, input_image );

  // Compute line that separes both feet
  TPoint p0 = global_ellipse->tPoint( TScalar( 0 ) );
  TPoint p1 = global_ellipse->tPoint( TScalar( vnl_math::pi ) );

  // Compute left moments
  TContour::Pointer left_ellipse;
  TVector left_cog;
  TMatrix left_inertia;
  ComputeMoments(
    left_ellipse, left_cog, left_inertia, input_image, &p0, &p1
    );
  TComplex left_center = left_ellipse->Coeff( 0 );
  left_ellipse->Scale( TScalar( 0.4 ) );
  left_ellipse->SetCoeff( 0, left_center );

  TScalar left_angle =
    std::atan2( left_inertia[ 1 ][ 0 ], left_inertia[ 0 ][ 0 ] );
  std::cout << "Left: " << std::endl;
  std::cout << "\tCOG   : " << left_cog << std::endl;
  std::cout << "\tAngle : " << left_angle << std::endl;

  // Compute right moments
  TContour::Pointer right_ellipse;
  TVector right_cog;
  TMatrix right_inertia;
  ComputeMoments(
    right_ellipse, right_cog, right_inertia, input_image, &p1, &p0
    );
  TComplex right_center = right_ellipse->Coeff( 0 );
  right_ellipse->Scale( TScalar( 0.4 ) );
  right_ellipse->SetCoeff( 0, right_center );

  TScalar right_angle =
    std::atan2( right_inertia[ 1 ][ 0 ], right_inertia[ 0 ][ 0 ] );
  std::cout << "Right: " << std::endl;
  std::cout << "\tCOG   : " << right_cog << std::endl;
  std::cout << "\tAngle : " << right_angle << std::endl;

  // Show image
  TITK2VTK::Pointer itk2vtk = TITK2VTK::New( );
  itk2vtk->SetInput( input_image );
  itk2vtk->Update( );
  double int_range[ 2 ];
  itk2vtk->GetOutput( )->GetScalarRange( int_range );

  // Create a greyscale lookup table
  TLookupTable input_image_table = TLookupTable::New( );
  input_image_table->SetRange( int_range[ 0 ], int_range[ 1 ] );
  input_image_table->SetValueRange( 0, 1 );
  input_image_table->SetSaturationRange( 0, 0 );
  input_image_table->SetRampToLinear( );
  input_image_table->Build( );

  // Map the image through the lookup table
  TImageMapToColors input_image_color = TImageMapToColors::New( );
  input_image_color->SetLookupTable( input_image_table );
  input_image_color->SetInput( itk2vtk->GetOutput( ) );

  // Create actor
  TImageActor input_image_actor = TImageActor::New( );
  input_image_actor->SetInput( input_image_color->GetOutput( ) );

  // Show global inertia
  TMatrix4x4 global_matrix = TMatrix4x4::New( );
  global_matrix->Identity( );
  global_matrix->SetElement( 0, 0, double( global_inertia[ 0 ][ 0 ] ) );
  global_matrix->SetElement( 1, 0, double( global_inertia[ 1 ][ 0 ] ) );
  global_matrix->SetElement( 0, 1, double( global_inertia[ 0 ][ 1 ] ) );
  global_matrix->SetElement( 1, 1, double( global_inertia[ 1 ][ 1 ] ) );
  global_matrix->SetElement( 0, 3, double( global_cog[ 0 ] ) );
  global_matrix->SetElement( 1, 3, double( global_cog[ 1 ] ) );

  TAxesActor global_axes = TAxesActor::New( );
  global_axes->SetUserMatrix( global_matrix );
  global_axes->SetConeResolution( 3 );
  global_axes->AxisLabelsOff( );
  global_axes->SetTotalLength( 70, 70, 0 );

  // Show left inertia
  TMatrix4x4 left_matrix = TMatrix4x4::New( );
  left_matrix->Identity( );
  left_matrix->SetElement( 0, 0, double( left_inertia[ 0 ][ 0 ] ) );
  left_matrix->SetElement( 1, 0, double( left_inertia[ 1 ][ 0 ] ) );
  left_matrix->SetElement( 0, 1, double( left_inertia[ 0 ][ 1 ] ) );
  left_matrix->SetElement( 1, 1, double( left_inertia[ 1 ][ 1 ] ) );
  left_matrix->SetElement( 0, 3, double( left_cog[ 0 ] ) );
  left_matrix->SetElement( 1, 3, double( left_cog[ 1 ] ) );

  TAxesActor left_axes = TAxesActor::New( );
  left_axes->SetUserMatrix( left_matrix );
  left_axes->SetConeResolution( 3 );
  left_axes->AxisLabelsOff( );
  left_axes->SetTotalLength( 35, 35, 0 );

  // Show right inertia
  TMatrix4x4 right_matrix = TMatrix4x4::New( );
  right_matrix->Identity( );
  right_matrix->SetElement( 0, 0, double( right_inertia[ 0 ][ 0 ] ) );
  right_matrix->SetElement( 1, 0, double( right_inertia[ 1 ][ 0 ] ) );
  right_matrix->SetElement( 0, 1, double( right_inertia[ 0 ][ 1 ] ) );
  right_matrix->SetElement( 1, 1, double( right_inertia[ 1 ][ 1 ] ) );
  right_matrix->SetElement( 0, 3, double( right_cog[ 0 ] ) );
  right_matrix->SetElement( 1, 3, double( right_cog[ 1 ] ) );

  TAxesActor right_axes = TAxesActor::New( );
  right_axes->SetUserMatrix( right_matrix );
  right_axes->SetConeResolution( 3 );
  right_axes->AxisLabelsOff( );
  right_axes->SetTotalLength( 35, 35, 0 );

  // Show ellipses
  TContourActor left_ellipse_actor = TContourActor::New( );
  left_ellipse_actor->SetSampling( 1000 );
  left_ellipse_actor->SetContour( left_ellipse.GetPointer( ) );
  left_ellipse_actor->GetProperty( )->SetColor( 0, 1, 0 );

  TContourActor right_ellipse_actor = TContourActor::New( );
  right_ellipse_actor->SetSampling( 1000 );
  right_ellipse_actor->SetContour( right_ellipse.GetPointer( ) );
  right_ellipse_actor->GetProperty( )->SetColor( 0, 0, 1 );

  // Show all
  TRenderer renderer = TRenderer::New( );
  renderer->SetBackground( 0.7, 0.7, 0.7 );
  renderer->AddActor( input_image_actor );
  renderer->AddActor( global_axes );
  renderer->AddActor( left_axes );
  renderer->AddActor( right_axes );
  renderer->AddActor( left_ellipse_actor );
  renderer->AddActor( right_ellipse_actor );

  TRenderWindow window = TRenderWindow::New( );
  window->AddRenderer( renderer );

  TInteractor interactor = TInteractor::New( );
  window->SetInteractor( interactor );
  window->Render( );
  interactor->Start( );

  double radians = angle * double( vnl_math::pi ) / double( 180 );

  TImage::Pointer left_cut, right_cut;
  CutImage( left_cut, input_image, left_ellipse, p0, p1, radians );
  CutImage( right_cut, input_image, right_ellipse, p1, p0, -radians );

  typedef itk::AddImageFilter< TImage, TImage, TImage > TAdd;
  TAdd::Pointer add = TAdd::New( );
  add->SetInput1( left_cut );
  add->SetInput2( right_cut );

  TImageWriter::Pointer otuput_image_writer = TImageWriter::New( );
  otuput_image_writer->SetInput( add->GetOutput( ) );
  otuput_image_writer->SetFileName( output_image_file );
  otuput_image_writer->Update( );

  return( 0 );
}

// -------------------------------------------------------------------------
void ComputeMoments(
  TContour::Pointer& ellipse, TVector& cog, TMatrix& inertia,
  const TImage* image, const TPoint* p0, const TPoint* p1
  )
{
  TMoments::Pointer moments = TMoments::New( );
  moments->SetImage( image );

  if( p0 != NULL && p1 != NULL )
  {
    TLine::Pointer line = TLine::New( );
    line->SetPoint0( *p0 );
    line->SetPoint1( *p1 );
    moments->SetSpatialMask( line );

  } // fi
  moments->Compute( );

  cog = moments->GetCenter( );
  inertia = moments->GetProperMatrix( );

  // Configure ellipse
  TComplex zn, zc, zp;
  moments->GetFourierEllipse( zn, zc, zp );
  ellipse = TContour::New( );
  ellipse->SetHarmonics( 1 );
  ellipse->SetCoeff( -1, zn );
  ellipse->SetCoeff(  0, zc );
  ellipse->SetCoeff(  1, zp );
}

// -------------------------------------------------------------------------
void CutImage(
  TImage::Pointer& cut,
  const TImage* image, const TContour* ellipse,
  const TPoint& p0, const TPoint& p1, double angle
  )
{
  typedef itk::ImageRegionConstIteratorWithIndex< TImage > TConstIt;
  typedef itk::ImageRegionIteratorWithIndex< TImage > TIt;

  TLine::Pointer line = TLine::New( );
  line->SetPoint0( p0 );
  line->SetPoint1( p1 );

  TImage::Pointer aux = TImage::New( );
  aux->SetBufferedRegion( image->GetBufferedRegion( ) );
  aux->SetRequestedRegion( image->GetRequestedRegion( ) );
  aux->SetLargestPossibleRegion( image->GetLargestPossibleRegion( ) );
  aux->SetOrigin( image->GetOrigin( ) );
  aux->SetSpacing( image->GetSpacing( ) );
  aux->Allocate( );

  TConstIt cIt( image, image->GetRequestedRegion( ) );
  TIt iIt( aux, aux->GetRequestedRegion( ) );
  cIt.GoToBegin( );
  iIt.GoToBegin( );
  for( ; !cIt.IsAtEnd( ) && !iIt.IsAtEnd( ); ++cIt, ++iIt )
  {
    TImage::IndexType idx = iIt.GetIndex( );
    TImage::PointType pnt;
    image->TransformIndexToPhysicalPoint( idx, pnt );
    if( line->IsInside( pnt ) )
      iIt.Set( cIt.Get( ) );
    else
      iIt.Set( TPixel( 0 ) );

  } // rof


  TTransform::InputPointType trCnt;
  trCnt[ 0 ] = std::real( ellipse->Coeff( 0 ) );
  trCnt[ 1 ] = std::imag( ellipse->Coeff( 0 ) );

  TVector v = ellipse->tPoint( 0 ) - ellipse->Center( );

  TTransform::Pointer tr = TTransform::New( );
  tr->SetCenter( trCnt );
  tr->SetAngle( std::atan2( v[ 1 ], v[ 0 ] ) + angle );

  TLinearInt::Pointer interpolator = TLinearInt::New( );

  TImage::SizeType size = image->GetBufferedRegion( ).GetSize( );
  size[ 0 ] *= 2;
  size[ 1 ] *= 2;
  
  TResample::Pointer resample = TResample::New( );
  resample->SetInput( aux );
  resample->SetDefaultPixelValue( TPixel( 0 ) );
  resample->SetInterpolator( interpolator );
  resample->SetTransform( tr );
  resample->SetOutputSpacing( image->GetSpacing( ) );
  resample->SetOutputDirection( image->GetDirection( ) );
  resample->SetOutputOrigin( image->GetOrigin( ) );
  resample->SetSize( size );
  resample->Update( );

  cut = TImage::New( );
  cut->Graft( resample->GetOutput( ) );
}

// eof - $RCSfile$
